#include "MyProject.h"
#include "i2c0.c"
#include "i2c.c"

/******************************************************************************/
long  cpr0;
long  cpr;
float full_rotation_offset0;
float full_rotation_offset;
long  angle_data_prev0;
long  angle_data_prev;
unsigned long velocity_calc_timestamp0;
unsigned long velocity_calc_timestamp;
float angle_prev0;
float angle_prev;
/******************************************************************************/

/******************************************************************************/
#define  AS5600_Address  0x36
#define  RAW_Angle_Hi    0x0C   //V2.1.1 bugfix
//#define  RAW_Angle_Lo    0x0D
#define  AS5600_CPR      4096
/******************************************************************************/
u16 I2C_getRawCount0()
{
	return AS5600_ReadRawAngleTwo0();
}
u16 I2C_getRawCount()
{
	return AS5600_ReadRawAngleTwo();
}
/******************************************************************************/

/******************************************************************************/
void MagneticSensor_Init0(void)
{
	cpr0=AS5600_CPR;
	angle_data_prev0 = I2C_getRawCount0();  
	
	full_rotation_offset0 = 0;
	velocity_calc_timestamp0=0;
}
void MagneticSensor_Init(void)
{
	cpr=AS5600_CPR;
	angle_data_prev = I2C_getRawCount();  
	
	full_rotation_offset = 0;
	velocity_calc_timestamp=0;
}
/******************************************************************************/
float getAngle0(void)
{
	float angle_data0,d_angle0;
	
	angle_data0 = I2C_getRawCount0();
	
	// tracking the number of rotations 
	// in order to expand angle range form [0,2PI] to basically infinity
	d_angle0 = angle_data0 - angle_data_prev0;
	// if overflow happened track it as full rotation
	if(fabs(d_angle0) > (0.8*cpr0) ) full_rotation_offset0 += d_angle0 > 0 ? -_2PI : _2PI; 
	// save the current angle value for the next steps
	// in order to know if overflow happened
	angle_data_prev0 = angle_data0;
	// return the full angle 
	// (number of full rotations)*2PI + current sensor angle 
	return  (full_rotation_offset0 + ( angle_data0 / (float)cpr0) * _2PI) ;
}
float getAngle(void)
{
	float angle_data,d_angle;
	
	angle_data = I2C_getRawCount();
	
	// tracking the number of rotations 
	// in order to expand angle range form [0,2PI] to basically infinity
	d_angle = angle_data - angle_data_prev;
	// if overflow happened track it as full rotation
	if(fabs(d_angle) > (0.8*cpr) ) full_rotation_offset += d_angle > 0 ? -_2PI : _2PI; 
	// save the current angle value for the next steps
	// in order to know if overflow happened
	angle_data_prev = angle_data;
	// return the full angle 
	// (number of full rotations)*2PI + current sensor angle 
	return  (full_rotation_offset + ( angle_data / (float)cpr) * _2PI) ;
}
/******************************************************************************/
// Shaft velocity calculation
float getVelocity0(void)
{
	unsigned long now_us;
	float Ts, angle_c, vel;

	// calculate sample time
	now_us = SysTick->VAL; //_micros();
	if(now_us<velocity_calc_timestamp0)Ts = (float)(velocity_calc_timestamp0 - now_us)/9*1e-6;
	else
		Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp0)/9*1e-6;
	// quick fix for strange cases (micros overflow)
	if(Ts == 0 || Ts > 0.5) Ts = 1e-3;

	// current angle
	angle_c = getAngle0();
	// velocity calculation
	vel = (angle_c - angle_prev0)/Ts;

	// save variables for future pass
	angle_prev0 = angle_c;
	velocity_calc_timestamp0 = now_us;
	return vel;
}
float getVelocity(void)
{
	unsigned long now_us;
	float Ts, angle_c, vel;

	// calculate sample time
	now_us = SysTick->VAL; //_micros();
	if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6;
	else
		Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/9*1e-6;
	// quick fix for strange cases (micros overflow)
	if(Ts == 0 || Ts > 0.5) Ts = 1e-3;

	// current angle
	angle_c = getAngle();
	// velocity calculation
	vel = (angle_c - angle_prev)/Ts;

	// save variables for future pass
	angle_prev = angle_c;
	velocity_calc_timestamp = now_us;
	return vel;
}
/******************************************************************************/



